package endcourseproject;


import lejos.nxt.LCD;
import lejos.nxt.UltrasonicSensor;


public class UltraAvoid extends Behavior {

	private UltrasonicSensor us = null;
	private DirectionSensor ds = null;
	
	public UltraAvoid(UltrasonicSensor us, DirectionSensor ds) {
		this.us = us;
		this.ds = ds;
	}

	private int _direction = 0;
	private float _magnitude = 0;
	
	@Override
	public synchronized int getDirection() {
		return _direction;
	}
	
	@Override
	public synchronized float getMagnitude() {
		return _magnitude;
	}

	private synchronized void set(int direction, float magnitude)
	{
		_direction = direction;
		_magnitude = magnitude;
	}
	
	private final int THRESHOLD = 40;
	private static final float AVOID_MAGNITUDE = 10;
	
	
	private enum AvoidState
	{
		IDLE,
		AVOID_BEGIN,
		AVOIDING
	}
		
	public void run()
	{
		final int TIMEOUT_CNT = 75; // times 100 ms
		int value = 0;
		int direction = 0;
		int magnitude = 0;
		AvoidState state = AvoidState.IDLE;
		int avoid_timeout = 0;
		
		while(true)
		{		
/*
			Common.sleep(100);
			value = us.getDistance();
*/			Common.sleep(25);
			value = us.getDistance();
			Common.sleep(25);
			value += us.getDistance();
			Common.sleep(25);
			value += us.getDistance();
			Common.sleep(25);
			value += us.getDistance();
			value = value/4;

			//LCD.drawString("dist:"+value+"  t:"+avoid_timeout+"  ", 0, 5);
			
			switch(state)
			{
				case IDLE:
					if(value < THRESHOLD)
					{
						state = AvoidState.AVOID_BEGIN;
					}
					else
					{
						set(direction, 0);
					}
					break;
				case AVOID_BEGIN:
					//Common.beep();
					
					direction = ds.getDirection();	
					direction = direction + 120; // TODO make const - could also be -120

					if( direction >=360) direction -=360;

					avoid_timeout = TIMEOUT_CNT;
					if(value < THRESHOLD)
					{
						magnitude = (int)(AVOID_MAGNITUDE * ((float)THRESHOLD / (float)(THRESHOLD - value))); 
					}
					else{
						magnitude = (int)(AVOID_MAGNITUDE * ((float)avoid_timeout/(float)TIMEOUT_CNT)); 
					}	
					set(direction, magnitude);
					state = AvoidState.AVOIDING;
					
					break;
				case AVOIDING:
					if(value > THRESHOLD)
					{
						if( avoid_timeout-- > 0)
						{
							magnitude = (int)(AVOID_MAGNITUDE * ((float)avoid_timeout/(float)TIMEOUT_CNT)); 
						}
						else
						{
							state = AvoidState.IDLE;
						}
					}
					else
					{
						state = AvoidState.AVOID_BEGIN;
					}
					
					set(direction, magnitude);
					break;
				default:
					state = AvoidState.IDLE;
					break;
				
			}

		}
	}
	
}
